Control Theory¶
This notebook explore the dynamics and control of the cruise control system of a vehicle.
Cruise Control System¶
The Cruise Control is a system that automatically controls the speed of a motor vehicle. Applying some simplifications, we can describe the systems with the following block diagram.

Where the User inputs a desired Velocity [km/h]. This value is translated into bits [b] so it can be read by the Engine Control Unit (ECU). Where a compensator signals the Electric Throttle Controller (ETC) to determine a percentage [0-100%]. This signal is also in bits, so there should be a component that converts the signal into voltage [V] to control the Motor, this motor produces a driving force that impulses the vehicle forwards.
Now, there are external forces that act against the vehicle's motion. We call these forces Disturbances. The sum of these forces is the disturbance force, $F_{d}$. We can make a process out of Newton's second law, $F=ma$ to explain the Vehicle's dynamics. Where the final velocity can be obtained from the integral of the acceleration.
The feedback signal results from a Speedometer readings (which can also be affected), there should be also a conversor so the signal error can be generated from the difference between the desired and the actual velocity.
Dynamics¶
While this does not intend to be a comprehensive vehicle dynamics model, we can use a simplified model to simulate the behavior of a vehicle. The vehicle produces a force that propels it forward. This force, $F$, represents the vehicle's engine generated force.
We will define this force as:
$$F = \alpha \ T_{m} \ [1 - \beta \ (\frac{\omega}{\omega_{m}} - 1)^2] \ t$$
where:
- $\alpha$ is the gear ratio / wheel radius [1/m]
- $T_{m}$ is the maximum torque [Nm]
- $\beta$ is the peak engine rolloff [dimensionless]
- $\omega$ is the angular velocity [rad/s]
- $\omega_{m}$ is the vehicle's maximum angular velocity [rad/s]
- $t$ is the throttle input, a value between $0$ and $1$ [dimensionless]
Those values can be found in the vehicle's specifications.
This law also states that the acceleration of an object is directly proportional to the net force acting on it and inversely proportional to its mass. The law is given by the equation:
$$F = m \ a$$
For our vehicle model, the net force is the sum of the throttle force and the disturbance forces:
$$F_{net} = F - F_{d}$$
Then, the acceleration is given by:
$$a = \frac{F_{net}}{m}$$
Disturbance Forces¶
We will take into account three major forces that act against the vehicle's motion. The sum of these forces will the disturbance force, $F_{d}$.
$$F_{d} = F_{g} + F_{r} + F_{a}$$
Gravity¶
The force of gravity is given by:
$$F_{g} = m \ g \ sin(\theta)$$
where:
- $m$ is the mass of the vehicle
- $g$ is the acceleration due to gravity
- $\theta$ is the elevation angle of the road
Rolling Friction¶
Rolling friction is the force that opposes the motion of the vehicle. It is given by:
$$F_{r} = \mu \ N$$
where:
- $\mu$ is the coefficient of rolling friction
- $N$ is the normal force
Aerodynamic Drag Force¶
Drag depends on the properties of the fluid and on the size, shape, and speed of the object. One way to express this is by means of the drag equation:
$$F_{a} = \frac{1}{2} \ c_{d} \ A \ \rho \ v^2$$
where:
- $F_{d}$ is the drag force
- $\rho$ is the density of the fluid
- $v$ is the speed of the object relative to the fluid
- $A$ is the area of the object facing the fluid
- $c_{d}$ is the drag coefficient - a dimensionless number
As we will evaluate aerodynamics, the 'fluid' is the air.
At $101.325\ kPa$ (abs) and $15 °C$ ($59 °F$), air has a density of approximately $1.225 kg/m^3$ according to the International Standard Atmosphere (ISA).
Typical Drag Coefficients values¶
| Object | Drag Coefficient ($c_{d}$) |
|---|---|
| Airfoil | 0.05 |
| Toyota Camry | 0.28 |
| Ford Focus | 0.32 |
| Honda Civic | 0.36 |
| Ferrari Testarossa | 0.37 |
| Dodge Ram pickup | 0.43 |
| Bicycle | 0.9 |
Source: College Physics, OpenStax
from math import copysign, sin
import numpy as np
import pandas as pd
import seaborn as sns
from matplotlib import pyplot as plt
from pydantic import BaseModel
# Constants
air_density = 1.225 # kg/m^3
g = 9.80665 # m/s^2
Vehicle Model¶
We can model a simplified vehicle dynamics using a python class.
class Vehicle(BaseModel):
"""
Vehicle Dynamics Model
Attributes:
position: current position of the vehicle (m)
speed: current speed of the vehicle (m/s)
mass: mass of the vehicle (Kg)
max_throttle: maximum throttle force (N)
drag_coefficient: drag coefficient (dimensionless)
frontal_area: frontal area of the vehicle (m^2)
"""
position: float = 0 # m
speed: float = 0 # m/s
mass: float # Kg
max_throttle: float # N
drag_coefficient: float # dimensionless
frontal_area: float # m^2
Toyota Camry Example¶

Toyota Camry XSE 2025 (4T1DAACK2SU017407)
Specs:
2.5-Liter, 4-cylinder DOHC 16-valve, Variable Valve Timing-intelligence (VVT-i) with Variable Valve Timing-intelligence by Electric motor(VVT-iE) on the intake side, 183 hp @ 6000 rpm, 163 lb.-ft. @ 3600-5200 rpm: 2.5-Liter, 4-cylinder DOHC 16-valve, Variable Valve Timing-intelligence (VVT-i) with Variable Valve Timing-intelligence by Electric motor(VVT-iE) on the intake side, 183 hp @ 6000 rpm, 163 lb.-ft. @ 3600-5200 rpm
According to the Toyota specifications, this unit drag coefficient ($c_{d}$) is approximately $0.37$ and the frontal area ($A$) is $1.94 m^2$. It weighs $3538 \ lbs$ ($1604 \ kg$) and has a maximum horsepower of $184hp$ ($137209 \ W$), and maximum top speed of $130mph$ ($209.2km/h$ or $58.33 m/s$). It's maximum torque $T_{m}$ is $163 \ lb.-ft.$ ($221 \ Nm$), and peak engine angular velocity $\omega_{m}$ is $5200 \ rpm$ ($545.3 \ rad/s$).
camry = Vehicle(mass=1604, max_throttle=2352, drag_coefficient=0.37, frontal_area=1.94)
print(f"Toyota Camry XSE 2025({camry})")
Toyota Camry XSE 2025(position=0 speed=0 mass=1604.0 max_throttle=2352.0 drag_coefficient=0.37 frontal_area=1.94)
For our process or plant, we will use a simple model that simulates the vehicle dynamics. This model will have a method that updates the vehicle's position and speed based on the throttle input and the time step.
from math import radians
# Sign function
sign = lambda x: copysign(1, x)
def process(vehicle: Vehicle, throttle: float, dt: float, theta: float = 0.0, mu: float = 0.01) -> float:
"""
Simulate vehicle dynamics updating its position and speed.
Args:
vehicle: a dynamic model
throttle: percentage of throttle input
dt: time step
theta: inclination angle of the road
mu: coefficient of rolling friction
Returns:
vehicle speed
"""
m = vehicle.mass # Kg
v = vehicle.speed # m/s
area = vehicle.frontal_area # m^2
# Force generated by the throttle input
f = throttle * vehicle.max_throttle
# force Fg = m g sin \theta.
fg = m * g * sin(radians(theta))
# A simple model of rolling friction is Fr = m g Cr sgn(v), where Cr is
# the coefficient of rolling friction and sgn(v) is the sign of v (±1) or
# zero if v = 0.
fr = m * g * mu * sign(v)
# Fa = 1/2 * Cd * A * rho * v^2
fa = 0.5 * vehicle.drag_coefficient * area * air_density * v ** 2
# Total disturbance force
fd = fg + fr + fa
# a = F/m
a = (f - fd) / m
# Update vehicle position and speed
vehicle.position += v * dt
vehicle.speed += a * dt
if vehicle.speed < 0:
vehicle.speed = 0
return vehicle.speed
We have built now a simple open-loop model of the vehicle dynamics.

class Simulation(BaseModel):
times: list[float] = []
speeds: list[float] = []
positions: list[float] = []
errors: list[float] = []
inclinations: list[float] = []
def simulate(vehicle: Vehicle, total_time: float = 3_600.0, dt: float = 0.1) -> Simulation:
sim = Simulation()
# Reset vehicle state
vehicle.position = 0
vehicle.speed = 0
for t in np.arange(0, total_time, dt):
throttle = 0.75 # %
speed = process(vehicle, throttle, dt)
# Store simulation data
sim.times.append(t)
sim.speeds.append(speed)
sim.positions.append(vehicle.position)
return sim
def plot_results(simulation: Simulation, set_speed: float = 0.0):
plt.clf()
plt.close()
plt.figure(figsize=(12, 6))
plt.subplot(3, 1, 1)
plt.plot(simulation.times, simulation.speeds, label='Speed')
if set_speed > 0.0:
plt.axhline(y=set_speed, color='r', linestyle='--', label='Set Speed')
plt.xlabel('Time (s)')
plt.ylabel('Speed (m/s)')
plt.title('Vehicle Speed Over Time')
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(simulation.times, simulation.positions, label='Position')
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.title('Vehicle Position Over Time')
plt.legend()
if len(simulation.errors) > 0:
plt.subplot(3, 1, 3)
plt.plot(simulation.times, simulation.errors, label='Error')
plt.xlabel('Time (s)')
plt.ylabel('Error (m/s)')
plt.title('Speed Error Over Time')
plt.legend()
plt.tight_layout()
plt.show()
open_loop = simulate(camry)
plot_results(open_loop)
Nothing surprising here, as no control is applied, the vehicle accelerates until it reaches a maximum speed according to the throttle input and the disturbance forces acting on it.
Let's now implement a simple closed-loop system to control the speed of the vehicle.

Where the controller:

class PIDController(BaseModel):
"""
Proportional-Integral-Derivative Controller
Attributes:
kp: proportional gain
ki: integral gain
kd: derivative gain
"""
kp: float
ki: float = 0.0
kd: float = 0.0
_integral: float = 0
_previous_error: float = 0
def update(self, error: float, dt: float) -> float:
"""
Update PID controller output
Args:
error: signal error
dt: time step
Returns:
A value between [-1, 1] control signal
"""
self._integral += error * dt
derivative = (error - self._previous_error) / dt
# u(t) = Kp * e(t) + Ki * ā«e(t)dt + Kd * de(t)/dt
output = self.kp * error + self.ki * self._integral + self.kd * derivative
self._previous_error = error
return np.clip(output, -1, 1)
Update the simulation function to include the controller.
from typing import Callable
def simulate(vehicle: Vehicle,
set_speed: float,
controller: Callable[[float, float], float],
total_time: float = 3_600.0,
dt: float = 0.1) -> Simulation:
sim = Simulation()
vehicle.speed = 0
vehicle.position = 0
for t in np.arange(0, total_time, dt):
error = set_speed - vehicle.speed
throttle = controller(error, dt) # %
speed = process(vehicle, throttle, dt)
# Store simulation data
sim.times.append(t)
sim.speeds.append(speed)
sim.positions.append(vehicle.position)
sim.errors.append(error)
return sim
Comparing Controllers¶
We will compare the performance of the controllers by setting a desired speed of $30 \ m/s$ (108 km/h) and evaluating the vehicle's response.
def calculate_settling_time(times: list, output: list, percentage: float = 0.05):
"""
Calculate the settling time from the system output.
Parameters:
times (list): Recorded time of the system.
output (list): System output.
percentage (float): Tolerance percentage, by default 5%.
Returns:
float: settling time
"""
final_value = output[-1]
# bounds for 5% tolerance
lower_bound = final_value * (1 - percentage)
upper_bound = final_value * (1 + percentage)
# find the first index where the output is within the bounds
for i in reversed(range(len(output))):
if not lower_bound <= output[i] <= upper_bound:
# the last time the output was outside bounds + one step (we left the bounds)
return times[i + 1]
# output was always within the bounds
return times[0]
p = PIDController(kp=1)
desired_speed = 30 # m/s (108 km/h)
p_simulation = simulate(camry, set_speed=desired_speed, controller=p.update)
plot_results(p_simulation, set_speed=desired_speed)
pi = PIDController(kp=1, ki=0.5)
pi_simulation = simulate(camry, set_speed=desired_speed, controller=pi.update)
plot_results(pi_simulation, set_speed=desired_speed)
pid = PIDController(kp=1, ki=0.5, kd=2)
pid_simulation = simulate(camry, set_speed=desired_speed, controller=pid.update)
plot_results(pid_simulation, set_speed=desired_speed)
t_p = calculate_settling_time(p_simulation.times, p_simulation.speeds)
t_pi = calculate_settling_time(pi_simulation.times, pi_simulation.speeds)
t_pid = calculate_settling_time(pid_simulation.times, pid_simulation.speeds)
print(f"Settling Time (P): {t_p:.2f} s")
print(f"Settling Time (PI): {t_pi:.2f} s")
print(f"Settling Time (PID): {t_pid:.2f} s")
Settling Time (P): 21.80 s Settling Time (PI): 86.00 s Settling Time (PID): 97.00 s
Disturbance¶
Let's add a disturbance to the system and evaluate the controllers' performance.
We will simulate a sudden change in the road inclination angle.
from scipy.stats import semicircular
def produce_inclination(prev_theta: float = 0.0,
min_theta: float = -5.0,
max_theta: float = 20.0,
) -> float:
# Random change in the inclination angle
change = semicircular.rvs()
# Update the inclination angle limiting its value
theta = round(prev_theta + change, 2)
return np.clip(theta, min_theta, max_theta)
current_theta = 0.0
inclinations = []
for _ in range(500):
current_theta = produce_inclination(prev_theta=current_theta)
inclinations.append(current_theta)
# Plotting
sns.histplot(inclinations, bins=400, kde=True)
plt.xlabel('Inclination')
plt.ylabel('Frequency')
plt.title('Histogram of Generated Road Inclinations')
plt.show()
Now, these inclinations change do not happen at regular intervals, so we will update the simulation function to include the disturbance.
We will use Maxwell distribution to generate random intervals between disturbances.
$$f(x) = \sqrt{2/\pi} x^2 e^{-x^2/2}$$
from scipy.stats import maxwell
fig, ax = plt.subplots(1, 1)
rv = maxwell()
x = np.linspace(maxwell.ppf(0.01),
maxwell.ppf(0.99), 100)
ax.plot(x, maxwell.pdf(x), 'r-', lw=5, alpha=0.6, label='maxwell pdf')
ax.set_xlim([x[0], x[-1]])
plt.show()
def interval_generator() -> list[int]:
"""
Generate random intervals between disturbances.
"""
intervals = maxwell.rvs(loc=5, scale=750, size=3_600)
return list(set(map(int, sorted(intervals))))
Update the simulation function to include the disturbance.
def adjust_inclination(theta: float = 0.0,
rate: float = 0.5,
) -> float:
if theta >= 0:
return np.clip(theta - rate, 0, theta)
elif theta < 0:
return np.clip(theta + rate, theta, 0)
def generate_disturbances() -> dict:
"""
Generate random disturbances.
"""
times = interval_generator()
return {t: produce_inclination() for t in times}
road_inclinations = generate_disturbances()
def simulate_with_disturbance(vehicle: Vehicle,
set_speed: float,
controller: Callable[[float, float], float],
total_time: float = 3_600.0,
dt: float = 1.0) -> Simulation:
sim = Simulation()
# Set Initial State
vehicle.speed = 0.0
vehicle.position = 0.0
inclination_angle = 0.0
t_last_disturbance = 0
for t in np.arange(0, total_time, dt):
# Check for disturbances
if int(t) in road_inclinations.keys():
inclination_angle = road_inclinations[int(t)]
t_last_disturbance = t
# Gradual change in the inclination angle after a disturbance to simulate the road leveling
if t_last_disturbance + 2 > t:
inclination_angle = adjust_inclination(inclination_angle)
error = set_speed - vehicle.speed
throttle = controller(error, dt) # %
speed = process(vehicle, throttle, dt, theta=inclination_angle)
if speed > 70.0:
print(
f"SPEED Error at {t}s, speed: {speed} m/s, throttle: {throttle * 100} %, error: {error} m/s, inclination: {inclination_angle}")
break
# Store simulation data
sim.times.append(t)
sim.speeds.append(speed)
sim.positions.append(vehicle.position)
sim.errors.append(error)
return sim
p_simulation = simulate_with_disturbance(camry, set_speed=desired_speed, controller=p.update, total_time=45 * 60)
plot_results(p_simulation, set_speed=desired_speed)
pi_simulation = simulate_with_disturbance(camry, set_speed=desired_speed, controller=pi.update, total_time=45 * 60)
plot_results(pi_simulation, set_speed=desired_speed)
pid_simulation = simulate_with_disturbance(camry, set_speed=desired_speed, controller=pid.update, total_time=45 * 60)
plot_results(pid_simulation, set_speed=desired_speed)
data = {
"Simulation": ["P", "PI", "PID"],
"Max Distance (km)": [max(p_simulation.positions) / 1_000, max(pi_simulation.positions) / 1_000,
max(pid_simulation.positions) / 1_000],
"Average Speed": [sum(p_simulation.speeds) / len(p_simulation.speeds),
sum(pi_simulation.speeds) / len(pi_simulation.speeds),
sum(pid_simulation.speeds) / len(pid_simulation.speeds)],
"Max Speed": [max(p_simulation.speeds), max(pi_simulation.speeds), max(pid_simulation.speeds)],
"Min Speed": [min(p_simulation.speeds), min(pi_simulation.speeds), min(pid_simulation.speeds)],
"Max Error": [max(p_simulation.errors), max(pi_simulation.errors), max(pid_simulation.errors)],
"Min Error": [min(p_simulation.errors), min(pi_simulation.errors), min(pid_simulation.errors)]
}
pd.DataFrame(data)
| Simulation | Max Distance (km) | Average Speed | Max Speed | Min Speed | Max Error | Min Error | |
|---|---|---|---|---|---|---|---|
| 0 | P | 80.022734 | 29.649075 | 29.866562 | 1.368268 | 30.0 | 0.133438 |
| 1 | PI | 81.000000 | 30.011111 | 52.480769 | 1.368268 | 30.0 | -22.480769 |
| 2 | PID | 80.999246 | 30.010498 | 52.480769 | 1.368268 | 30.0 | -22.480769 |
Speedometer¶
The speedometer is a device that measures the instantaneous speed of a vehicle. While manufacturers have different technologies to measure speed, and calibrate their speedometers according to the vehicle's specifications, we can simulate a simple speedometer error as a percentage of the actual speed.
Let's simulate that the speedometer may have an error $+/- 2 km/h$, Which could translate to $0.5556 m/s$. For this simulation, we will consider a normal distribution for errors, and uniform random noise to simulate the speedometer's readings.
Also, Speedometers have a range of readings, so we will limit the readings between $0$ and $75 m/s$.
SPEEDOMETER_MIN = 0 # m/s
SPEEDOMETER_MAX = 75 # m/s
def measure_element(speed: float) -> float:
"""
Measure the element with a 98% accuracy.
"""
error = np.random.normal(0.2778, 0.5556)
random = np.random.choice([1, -1])
return np.clip(speed + random * error, SPEEDOMETER_MIN, SPEEDOMETER_MAX)
Let's see some examples of speedometer readings.
sample_measured_speeds = [measure_element(speed) for speed in p_simulation.speeds]
plt.figure(figsize=(12, 6))
plt.plot(p_simulation.times, p_simulation.speeds, label='Actual Speed')
plt.plot(p_simulation.times, sample_measured_speeds, label='Measured Speed')
plt.xlabel('Time (s)')
plt.ylabel('Speed (m/s)')
plt.title('Actual vs Measured Speed')
plt.legend()
plt.show()
speed_errors = [actual - measured for actual, measured in zip(p_simulation.speeds, sample_measured_speeds)]
plt.figure(figsize=(10, 6))
plt.plot(p_simulation.times, speed_errors)
plt.xlabel('Time (s)')
plt.ylabel('Speed Error (units as per speed)')
plt.title('Error in Speed Over Time')
plt.show()
plt.figure(figsize=(10, 6))
plt.hist(speed_errors, bins=30)
plt.xlabel('Speed Error (units as per speed)')
plt.ylabel('Frequency')
plt.title('Distribution of Speed Errors')
plt.show()
print(
f"Percentage of less readings than actual speed: {len([e for e in speed_errors if e < 0]) / len(speed_errors) * 100:.2f}%")
print(
f"Percentage of more readings than actual speed: {len([e for e in speed_errors if e > 0]) / len(speed_errors) * 100:.2f}%")
Percentage of less readings than actual speed: 49.89% Percentage of more readings than actual speed: 50.11%
df_speedometer = pd.DataFrame({
"Actual Speed": p_simulation.speeds,
"Measured Speed": sample_measured_speeds,
"Speed Error": speed_errors
})
df_speedometer.describe()
| Actual Speed | Measured Speed | Speed Error | |
|---|---|---|---|
| count | 2700.000000 | 2700.000000 | 2700.000000 |
| mean | 29.649075 | 29.649936 | -0.000862 |
| std | 1.498468 | 1.620539 | 0.611654 |
| min | 1.368268 | 1.753615 | -2.265078 |
| 25% | 29.763037 | 29.354752 | -0.392556 |
| 50% | 29.767485 | 29.760115 | 0.002619 |
| 75% | 29.771409 | 30.154742 | 0.402372 |
| max | 29.866562 | 31.970968 | 1.762274 |
df_speedometer.head()
| Actual Speed | Measured Speed | Speed Error | |
|---|---|---|---|
| 0 | 1.368268 | 2.524395 | -1.156127 |
| 1 | 2.736022 | 1.753615 | 0.982407 |
| 2 | 4.102238 | 4.332648 | -0.230410 |
| 3 | 5.465893 | 5.144739 | 0.321154 |
| 4 | 6.825972 | 6.327222 | 0.498750 |
Having the speedometer readings in mind, we can now say that our vehicle is in a stable state when the error is within the $4 km/h$ tolerance of the actual speed.
def ms_to_kmh(speed: float) -> float:
"""
Speed conversion
Args:
speed: speed in m/s
Returns:
speed in km/h
"""
return speed * 3.6
def simulate_with_readings(vehicle: Vehicle,
set_speed: float,
controller: Callable[[float, float], float],
total_time: float = 3_600.0,
dt: float = 1.0,
initial_speed: float = 0.0) -> Simulation:
sim = Simulation()
# Set Initial State
vehicle.speed = initial_speed
vehicle.position = 0.0
inclination_angle = 0.0
t_last_disturbance = 0
for t in np.arange(0, total_time, dt):
# Check for disturbances
if int(t) in road_inclinations.keys():
inclination_angle = road_inclinations[int(t)]
t_last_disturbance = t
# Gradual change in the inclination angle after a disturbance to simulate the road leveling
if t_last_disturbance + 2 > t:
inclination_angle = adjust_inclination(inclination_angle)
# [H] - Measure the speed with the Speedometer
read_speed = measure_element(vehicle.speed)
# [e] - Calculate the error signal
error = set_speed - read_speed
# [Gc] - Apply the controller
throttle = controller(error, dt) # %
# [Gp] - Process the vehicle dynamics
speed = process(vehicle, throttle, dt, theta=inclination_angle)
# Store simulation data
sim.times.append(t)
sim.speeds.append(speed)
sim.positions.append(vehicle.position)
sim.inclinations.append(inclination_angle)
sim.errors.append(error)
return sim
def to_km_simulation(sim: Simulation):
"""
Convert the simulation speeds to km/h.
Args:
sim: Simulation data
Returns:
Simulation data with speeds in km/h
"""
# Convert speeds to km/h
sim.speeds = [ms_to_kmh(speed) for speed in sim.speeds]
sim.errors = [ms_to_kmh(error) for error in sim.errors]
# convert positions to km
sim.positions = [pos / 1_000 for pos in sim.positions]
def plot_in_kmh(simulation: Simulation, set_speed: float = 0.0):
"""
Plot the simulation results.
Args:
simulation: Simulation data
set_speed: Speed in km/h
"""
plt.clf()
plt.close()
plt.figure(figsize=(20, 15))
plt.subplot(3, 1, 1)
plt.plot(simulation.times, simulation.speeds, label='Actual Speed')
if set_speed > 0.0:
plt.axhline(y=set_speed + 4, color='greenyellow', linestyle='dotted', label='Speed Upper Bound')
plt.axhline(y=set_speed, color='darkolivegreen', linestyle='--', label='Step Speed')
plt.axhline(y=set_speed - 4, color='greenyellow', linestyle='dotted', label='Speed Lower Bound')
plt.xlabel('Time (s)')
plt.ylabel('Speed (km/h)')
plt.title('Vehicle Speed Over Time')
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(simulation.times, simulation.inclinations, label='Īø inclination')
plt.xlabel('Time (s)')
plt.ylabel('Inclination (Degrees)')
plt.title('Road Inclination Over Time')
plt.legend()
if len(simulation.errors) > 0:
plt.subplot(3, 1, 3)
plt.plot(simulation.times, simulation.errors, label='Error')
plt.xlabel('Time (s)')
plt.ylabel('Error (km/h)')
plt.title('Speed Error Over Time')
plt.legend()
plt.tight_layout()
plt.show()
p_simulation = simulate_with_readings(camry, set_speed=desired_speed, controller=p.update, total_time=45 * 60)
to_km_simulation(p_simulation)
plot_in_kmh(p_simulation, set_speed=ms_to_kmh(desired_speed))
pi_simulation = simulate_with_readings(camry, set_speed=desired_speed, controller=pi.update, total_time=45 * 60)
to_km_simulation(pi_simulation)
plot_in_kmh(pi_simulation, set_speed=ms_to_kmh(desired_speed))
pid_simulation = simulate_with_readings(camry, set_speed=desired_speed, controller=pid.update, total_time=45 * 60)
to_km_simulation(pid_simulation)
plot_in_kmh(pid_simulation, set_speed=ms_to_kmh(desired_speed))
p_ctrl = PIDController(kp=1)
p_opt_sim = simulate_with_readings(camry, set_speed=desired_speed, controller=p_ctrl.update, total_time=45 * 60)
to_km_simulation(p_opt_sim)
plot_in_kmh(p_opt_sim, set_speed=ms_to_kmh(desired_speed))
pi_ctrl = PIDController(kp=3, ki=0.005)
pi_opt_sim = simulate_with_readings(camry, set_speed=desired_speed, controller=pi_ctrl.update, total_time=45 * 60)
to_km_simulation(pi_opt_sim)
plot_in_kmh(pi_opt_sim, set_speed=ms_to_kmh(desired_speed))
pid_ctrl = PIDController(kp=3, ki=0.005, kd=0.4)
pid_opt_sim = simulate_with_readings(camry, set_speed=desired_speed, controller=pid_ctrl.update, total_time=45 * 60)
to_km_simulation(pid_opt_sim)
plot_in_kmh(pid_opt_sim, set_speed=ms_to_kmh(desired_speed))
data = {
"Simulation": ["P", "PI", "PID", "P (Optimized)", "PI (Optimized)", "PID (Optimized)"],
"Max Distance (km)": [max(p_simulation.positions), max(pi_simulation.positions), max(pid_simulation.positions),
max(p_opt_sim.positions), max(pi_opt_sim.positions), max(pid_opt_sim.positions)],
"Average Speed": [sum(p_simulation.speeds) / len(p_simulation.speeds),
sum(pi_simulation.speeds) / len(pi_simulation.speeds),
sum(pid_simulation.speeds) / len(pid_simulation.speeds),
sum(p_opt_sim.speeds) / len(p_opt_sim.speeds),
sum(pi_opt_sim.speeds) / len(pi_opt_sim.speeds),
sum(pid_opt_sim.speeds) / len(pid_opt_sim.speeds)],
"Max Speed": [max(p_simulation.speeds), max(pi_simulation.speeds), max(pid_simulation.speeds),
max(p_opt_sim.speeds), max(pi_opt_sim.speeds), max(pid_opt_sim.speeds)],
"Max Error": [max(p_simulation.errors), max(pi_simulation.errors), max(pid_simulation.errors),
max(p_opt_sim.errors), max(pi_opt_sim.errors), max(pid_opt_sim.errors)],
"Min Error": [min(p_simulation.errors), min(pi_simulation.errors), min(pid_simulation.errors),
min(p_opt_sim.errors), min(pi_opt_sim.errors), min(pid_opt_sim.errors)],
"Avg Error": [sum(p_simulation.errors) / len(p_simulation.errors),
sum(pi_simulation.errors) / len(pi_simulation.errors),
sum(pid_simulation.errors) / len(pid_simulation.errors),
sum(p_opt_sim.errors) / len(p_opt_sim.errors),
sum(pi_opt_sim.errors) / len(pi_opt_sim.errors), sum(pid_opt_sim.errors) / len(pid_opt_sim.errors)]
}
df_opt = pd.DataFrame(data)
df_opt
| Simulation | Max Distance (km) | Average Speed | Max Speed | Max Error | Min Error | Avg Error | |
|---|---|---|---|---|---|---|---|
| 0 | P | 79.732363 | 106.349316 | 114.848624 | 107.109215 | -10.813715 | 1.662624 |
| 1 | PI | 81.037755 | 108.092624 | 186.657745 | 108.000000 | -83.092243 | 0.002243 |
| 2 | PID | 81.018188 | 108.062117 | 188.930768 | 108.000000 | -85.490061 | -0.004419 |
| 3 | P (Optimized) | 79.755058 | 106.379970 | 114.122368 | 108.000000 | -10.171372 | 1.654980 |
| 4 | PI (Optimized) | 80.847730 | 107.838082 | 116.807322 | 108.000000 | -12.779878 | 0.291494 |
| 5 | PID (Optimized) | 80.727313 | 107.676281 | 116.468743 | 106.765584 | -12.093140 | 0.379390 |
Control Analysis¶
Updating the simulation to use the refined package code.
from acc.utils.rv import RoadInclinationGenerator
from acc.model.control import EngineControlUnit
from acc.simulation import SimulationResult, run_simulation
from acc.model import process
def ctrl_simulation(
kp: float = 0.5,
ki: float = 0.2,
kd: float = 1.0,
windup_protection: bool = False,
vi: float = 30.0,
generate_inclinations: bool = False
) -> SimulationResult:
"""
Run a simulation with a PID controller.
"""
camry_xse_2025 = process.Vehicle(mass=1_604,
drag_coefficient=0.28,
frontal_area=1.94,
torque_max=221,
omega_max=545.3,
gear_speed_ranges=[(0, 10), (10, 30), (30, 50), (50, 70), (70, 100), (100, 130),
(130, 160),
(160, 200)])
ecu = EngineControlUnit(kp=kp, ki=ki, kd=kd, windup_protection=windup_protection)
return run_simulation(vehicle=camry_xse_2025,
vi=vi,
control=ecu,
inclination_generator=RoadInclinationGenerator() if generate_inclinations else None,
initial_speed=0)
Running the simulation with a PID controller we notice that the speed reaches the upper limit for a long period before approaching the set speed.
from acc.utils import plot
results = ctrl_simulation(kp=2, ki=0.5, kd=4)
plot.plot_results(results, step_speed=30 * 3.6)
results.df().describe()
| Error | Speed | Throttle | Speedometer | |
|---|---|---|---|---|
| count | 3600.000000 | 3600.000000 | 3600.000000 | 3600.000000 |
| mean | 0.009106 | 107.980746 | 0.976328 | 107.990894 |
| std | 8.232561 | 7.942230 | 0.184040 | 8.232561 |
| min | -7.705622 | 4.303984 | -1.000000 | 0.838604 |
| 25% | -2.907126 | 107.556374 | 1.000000 | 107.394665 |
| 50% | -0.933405 | 108.701037 | 1.000000 | 108.933405 |
| 75% | 0.605335 | 111.527989 | 1.000000 | 110.907126 |
| max | 107.161396 | 111.585574 | 1.000000 | 115.705622 |
Taking a loot at the controller output u(t). It is seen that for the exact same time that the speed reaches the upper limit, the controller output is at its maximum value. While the speed is inside the tolerance, is not quite the desired behavior.
from acc.simulation import SimulationResult
def time_to_steady_state(result: SimulationResult, step_speed=30 * 3.6):
"""
Calculate the time to reach a steady state.
Args:
result: Simulation result
step_speed: Step speed
Returns:
Time to reach a steady state
"""
t = -1
for i, speed in enumerate(result.speeds):
if speed >= step_speed - 4:
t = i
break
return t
print(f"Time to reach Steady State: {time_to_steady_state(results)} s")
Time to reach Steady State: 136 s
The integral problem¶
The integral control term adds up all the error amounts as time goes by. As long as a difference between $V_i$ and Vo exists, an error will continue accumulating. The faster the value accumulates, the more control output is added to the system since it indicates that we are far from the SP.
If it so happens that in a particular system, we cannot add enough control signal (throttle) to reach the $V_i$, the error will keep accumulating, thereās nothing that can be done.
If this happens, the error keeps building, and the internal calculation of the control system has produced an output of well over 100% of the full output signal. In our vehicle, the āthrottle signalā is telling you to push the pedal beyond the floor⦠But this is simply not possible.
The symptom of this effect is seen as a delay between the change in the throttle and the system response. For a while, the control signal is beyond its limit before returning to the range of real-world changes. It is a difficult symptom to diagnose because the reason for the delay is the past error accumulation driving the output too far in the present.
Solution¶
The simplest is to change the Integral term summation if the control output reaches 100%. If this output is reached, we know that the error will still accumulate, itās unavoidable. Weāre driving as fast as we can. In this case, it might be reasonable to halt the accumulation of the Integral term until the SP is changed again
Previous Controller¶
We accumulate the error in the integral term:
def control(error: float):
integral += error * dt
derivative = (error - self._previous_error) / dt
# u(t) = Kp * e(t) + Ki * ā«e(t)dt + Kd * de(t)/dt
output = self.kp * error + self.ki * self._integral + self.kd * derivative
self._previous_error = error
return np.clip(output, -1, 1)
Windup Protection¶
Adding an optional windup protection to the controller. If the output is maxed out either brake (-100%) or accelerate (100%), stop accumulating error.
def control(error: float, windup: bool):
derivative = (error - self._previous_error) / dt
# u(t) = Kp * e(t) + Ki * ā«e(t)dt + Kd * de(t)/dt
output = self.kp * error + self.ki * self._integral + self.kd * derivative
self._previous_error = error
if self.windup_protection:
# If output is maxed out either brake or accelerate, stop accumulating error
if not (output == -1 and error < 0) and not (output == 1 and error > 0):
self._integral += error * dt
else:
self._integral += error * dt
return np.clip(output, -1, 1)
results = ctrl_simulation(kp=2, ki=0.5, kd=1.7, windup_protection=True)
plot.plot_results(results, step_speed=30 * 3.6)
The vehicle reaches the set speed in a shorter time, and the control output is within the limits, as we can see sooner throttle changes.
results.df().describe()
| Error | Speed | Throttle | Speedometer | |
|---|---|---|---|---|
| count | 3600.000000 | 3600.000000 | 3600.000000 | 3600.000000 |
| mean | 3.019558 | 104.989875 | 0.945361 | 104.980442 |
| std | 7.572387 | 7.306318 | 0.225944 | 7.572387 |
| min | -2.241880 | 4.303984 | -1.000000 | 0.701817 |
| 25% | 1.092793 | 105.526554 | 1.000000 | 105.140010 |
| 50% | 1.960259 | 106.117637 | 1.000000 | 106.039741 |
| 75% | 2.859990 | 106.596135 | 1.000000 | 106.907207 |
| max | 107.298183 | 107.871560 | 1.000000 | 110.241880 |
Removing Integral control¶
With the issues caused by the integral term, one can think of removing the component...
results = ctrl_simulation(kp=1, ki=0, kd=0)
plot.plot_results(results, step_speed=30 * 3.6)
To no surprise, the vehicle never reaches the set speed, as without an accumulation of errors, it is never compensated in the steady state. The e(t) never reaches zero.
results.df().describe()
| Error | Speed | Throttle | Speedometer | |
|---|---|---|---|---|
| count | 3600.000000 | 3600.000000 | 3600.000000 | 3600.000000 |
| mean | 4.941723 | 103.082856 | 0.925511 | 103.058277 |
| std | 7.275457 | 7.009159 | 0.136840 | 7.275457 |
| min | 0.608632 | 4.303984 | 0.169064 | 0.000000 |
| 25% | 3.236269 | 103.843895 | 0.898964 | 103.276018 |
| 50% | 3.991878 | 104.075180 | 1.000000 | 104.008122 |
| 75% | 4.723982 | 104.291125 | 1.000000 | 104.763731 |
| max | 108.000000 | 105.031663 | 1.000000 | 107.391368 |